perm filename POINTY.PAL[PNT,HE]5 blob
sn#365526 filedate 1978-07-03 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00010 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00002 00002 .TITLE POINTY
C00005 00003 ROUTINE TO EXECUTE A MOVE WITH NO 'AL ROUTINES
C00009 00004 LOOK IF ANYONE HIT KEYBOARD, DO A WHERE, AND SEE IF MOTION WANTED
C00012 00005 ROUTINE TO DISPLAY CURRENT JOINT ANGLES
C00015 00006 ROUTINE TO DISPLAY CURRENT REFERENCE POWER SUPPLY READINGS
C00017 00007 SECTION TO READ FORCE WRIST AND RETURN INFORMATION TO PDP10
C00018 00008 FORCE SENSING/COMPLIANCE PATCH AREA
C00020 00009 SIMULTANEOUS HAND OPERATION
C00022 00010 LOCAL STORAGE AREA
C00028 ENDMK
C⊗;
.TITLE POINTY
TRANS ==1 ;SET TO 1 TO PRINT OUT EULER AS WELL AS JOINT ANGLES
HANDOP==0 ;=1 FOR SIMULTANEOUS HAND OPERATION
SNGSTP==0 ;CONDITIONAL ASSEMBLY FLAGS FOR ARM.PAL
DIAGY ==0
STDALN==1
TACCAL==0
NOYELW==1
TIMER ==0
ISLIN ==1
FRCDAT==0
.INSRT K1DEF.PAL[11,SYS]
.INSRT ALHEAD.PAL[AL,HE]
USRDM == USRTM ;So we use transparent mapping
USRIM == USRTM
. = USRORG
.INSRT ARM.PAL[AL,HE]
.INSRT BEJCZY.PAL[AL,HE]
.INSRT ARITH.PAL[AL,HE]
.INSRT IO.PAL[PNT,HE]
.IFNZ TRANS
.INSRT ARMSOL.PAL[al,he]
.INSRT EULER.PAL[AL,HE]
.IFF
UPDATE:
SOLVE: 0
.ENDC
;COMMUNICATION LINK FOR "TLKEF5", "WHERE" ROUTINE FOR THE PDP10 AND "TLKEF6"
MASLC5==110100 ;ADDRESS OF MASTER NUMBER TO SIGNAL PDP10 WE ARE ALIVE
TRNADR==110102 ;SAVE TRANSFORM DATA IN HERE
JTSADR==110162 ;SAVE JOINT ANGLES IN HERE
MASLC6==110220 ;ADDRESS OF MASTER NUMBER TO SIGNAL PDP10 THAT WE ARE READING WRIST
WSTADR==110222 ;SAVE WRIST INFO IN HERE
MASTR5==12121 ;CHECK NUMBER TO PDP10
MASTR6==10567 ;CHECK NUMBER TO PDP10
;COMMUNICATION LOCATION BETWEEN MOVE.SAI AND MOVE.PAL
MCLOC== 100000
MVDNDR== 77776 ;PUT MASTER NUMBER IN HERE WHEN MOVE IS DONE
;DEVICE MECHANISM BITS
YELARM== 1 ;MECHANISM BITS: YELLOW ARM, NOT INCLUDING HAND
YELHND== 2 ;YELLOW HAND
BLUARM== 4 ;BLUE ARM, NOT INCLUDING HAND
BLUHND== 10 ;BLUE HAND
;ROUTINE TO EXECUTE A MOVE WITH NO 'AL ROUTINES
POINTY: CLR JMPFRC ;NO FORCE COMPLIANCE
MOV #PSTART,JOBSA
JMP 1010
PSTART: MOV #DEVICE,R1 ;INITIALIZE BLUE ARM TO RUN
JSR PC,INTARM
TST R0 ;CHECK FOR ERRORS
BEQ PHEADR ;BRANCH IF NO INITIALIZATION ERROR
BADINT: MOV R0,STATSV ;SAVE STATUS WORD TO PRINT OUT DETAILED ERROR MESS
MOV #BADSTR,SG ;ELSE TYPE ERROR MESSAGE
JSR PC,TYPERR
TST KBIS ;CHECK IF ANYONE HIT VT05 KEYBOARD
BEQ TRYAGN
CLRB KBIR
DISMIS
TRYAGN: CMP #10,STATSV
BNE PSTART
JMP START3
PHEADR: JSR PC,PRTHDR ;PRINT HEADER INFORMATION
CLR VT05 ; RESET VT05 FLAG
FORK #LOOK,#MLOOP,#USRDM
PRTW: MOV #COMWH,SG ;JUMP TOP LINE
JSR PC,TYPSTR
MOV #IOBUF,SG ;START BUILDING OUTPUT STRING IN HERE
MOV #BTH,R2 ;GET POINTERS JOINT READINGS
MOV #7,R3 ;7 JOINTS IN ALL
JTCLP: LDF @(R2)+,AC0 ;GET JOINT ANGLE
JSR PC,CVF ;CONVERT JOINT ANGLE TO STRING
SOB R3,JTCLP
MOVB #15,(SG)+ ;PUT IN A CR LF
MOVB #12,(SG)+
MOV #9.,R0 ;CHANGE STRING FORMAT FOR INTEGER NUMBER
CLR R1
JSR PC,FORMAT
MOV #BSRVOS,R1 ;GET POINTERS TO SERVOS
MOV #7,R3 ;CONVERT THE ACTUAL POT READINGS
NEXTP: MOV (R1)+,R0
LDCIF POT(R0),AC0 ;GET A/D READING
TST POT(R0) ;CHECK IF ROUND UP OR DOWN
BLT RNDDN
ADDF #40000,AC0 ;ROUND OFF READING
BR .+6
RNDDN: SUBF #40000,AC0
JSR PC,CVF ;CONVERT TO ASC
SOB R3,NEXTP
JSR PC,RSTFOR
CLRB (SG)
MOV #IOBUF,SG ;TYPE OUT WHOLE STRING
JSR PC,TYPSTR
.IFNZ TRANS
MOV #TRNADR,R0 ;GET CURRENT TRANSFORM IN HERE
MOV #THPTR,R1 ;POINT TO THE CURRENT JOINT ANGLES
MOV #BLUARM,R2 ;USE SOLUTION FOR BLUE ARM
JSR PC,UPDATE ;COMPUTE THE CURRENT TRANSFORM
MOV #TRNADR,R0 ;COMPUTE THE EULER ANGLES
MOV #EANGLE,R1 ;PUT THEM IN HERE
JSR PC,EULER
MOV #IOBUF,SG ;CONVERT TO STRING FOR OUTPUT
MOVB #15,(SG)+ ;PUT IN A CR LF LF
MOVB #12,(SG)+
MOVB #12,(SG)+
MOV #6,R0
MOV #EANGLE,R1
EANGC: LDF (R1)+,AC0
JSR PC,CVF
SOB R0,EANGC ;REPEAT TILL DONE
CLRB (SG)
MOV #IOBUF,SG ;TYPE OUT THE EULER ANGLES
JSR PC,TYPSTR
MOV #BTH,R0 ;TRANSFER THE JOINT ANGLES TO PDP10
MOV #JTSADR,R1 ; COMMUNICATIONS TABLE
MOV #7,R2 ;6 JOINTS + HAND
RHTLP: LDF @(R0)+,AC0
STF AC0,(R1)+
SOB R2,RHTLP
MOV #MASTR5,MASLC5 ;INDICATE TO PDP10 THAT "WHERE" IS WORKING
.ENDC
TST PRTDVB ;PRINT OUT DEVICE BLOCK?
BEQ $2
JSR PC,PRTDB ;YES, JUMP TO RELEVANT ROUTINE
$2: TST VT05
BEQ $3
DISMIS
$3: JMP PRTW
; LOOK IF ANYONE HIT KEYBOARD, DO A WHERE, AND SEE IF MOTION WANTED
LOOK: PDBLK 2,100,FP ; SET UP PBD FOR PROCESS THAT LOOKS AT MASTER NO
; READS ARM POSITIONS AND CHECKS IF ANYTHING FROM 10
MLOOP: TST KBIS ;CHECK IF ANYONE HIT VT05 KEYBOARD
BEQ ELFRDY
CLRB KBIR
CLR MASLC5 ;INDICATE "WHERE" NOT ACTIVE
CLR MASLC6 ;INDICATE "FORCE" NOT ACTIVE
MOV #1,VT05 ;INDICATE INTERRUPT FROM VT05
DISMIS
ELFRDY: MOV #MASTR5,MASLC5 ;INDICATE TO PDP10 THAT "WHERE" IS WORKING
; THIS STATEMENT INSERTED BECAUSE MASLC5 WASN'T
; GETTING UPDATED OFTEN ENOUGH
; ELFRDY: USED TO BE LABEL FOR THE NEXT INSTRUCTION
CMP #54321,MCLOC ;CHECK IF DATA BLOCK ARRIVED FROM 10
BEQ MOTION
CMP #MASTR6,MASLC6 ;CHECK IF TLKEF6 HAS RESET WORD
BEQ DOWHER
JSR PC,FORCE ;READ THE WRIST
MOV #MASTR6,MASLC6 ;UPDATE MASTER NUMBER
DOWHER: MOV #12345,MCLOC ;INDICATE ELF READY FOR DATA BLOCK
MOV #BARM+BHAND,WLST ;DO A WHERE FOR THE BLUE ARM
MOV #WLST,R0
MOV #DEVICE,R1
JSR PC,WHERE ;READ THE JOINT POSITIONS
TST R0 ;CHECK IF ANY ERRORS
BEQ $1 ;BRANCH IF OK
MOV R0,STATSV
MOV #BADW,SG ;ELSE TYPE ERROR MESSAGE
JSR PC,TYPERR
$1: SLEEP #20 ; GO TO SLEEP FOR 20 MSECS
BR MLOOP
;START OF MOVE CODE
MOTION: MOV #12345,MCLOC ;INDICATE ELF ALIVE FOR DATA BLOCK
MOV #MCLOC+4,R0 ;POINT TO COEFFICIENT LIST
MOV #DEVICM,R1 ;POINT TO DEVICE LIST
MOV #0,MVDNDR ;BEGINNING TO MOVE AGAIN
CMP #76,MCLOC+2 ;MOVE OR CENTER?
BNE CENCOD
JSR PC,MOVE
JMP CENCD2
CENCOD: JSR PC,CENTER
CENCD2: MOV #1,PRTDVB ;PRINT OUT NEW DEVICE BLOCK
MOV R0,STATSV ;SAVE STATUS WORD
MOV #12345,MVDNDR ;TELL 10 MOVE OVER
JMP MLOOP
;ROUTINE TO DISPLAY CURRENT JOINT ANGLES
;START OF ROUTINE TO PRINT WHERE LINES
TYPERR: JSR PC,TYPSTR
MOV #IOBUF,SG ;TYPE ERROR CODE
JSR PC,CVO
MOV #IOBUF,SG
JSR PC,TYPSTR
JSR PC,CRLF
JSR PC,EDETLS ;TYPE OUT DETAILS OF ERROR
CLR MASLC5 ;INDICATE W NOT WORKING
RTS PC
PRTHDR: MOV #COMHD,SG ;GO TO TOP OF PAGE
JSR PC,TYPSTR
MOV #HEADER,SG ;TYPE OUT COLUMN HEADER
JSR PC,TYPSTR
MOV #8.,R0
MOV #2,R1
JSR PC,FORMAT
RTS PC
PRTDB: CLR PRTDVB
CMP #0,STATSV
BNE $4
MOV #CLRSCR,SG
JSR PC,TYPSTR
MOV #SUCCES,SG ;SUCCESSFUL MOVE
JSR PC,TYPSTR ;PRINT RELEVANT MESSAGE
RTS PC
$4: MOV #NOSCCS,SG ;TOO BAD UNSUCCESSFUL MOVE
JSR PC,TYPSTR
MOV #CLRSCR,SG
JSR PC,TYPSTR
EDETLS: MOV STATSV,R0
CMP #15,R0 ;LOOK AT LOW BYTE ERRORS
BLT $5
ASL R0 ;THIS IS LOW BYTE ERROR
MOV ERRMES(R0),SG
JSR PC,TYPSTR ;PRINT OUT RELEVANT MESSAGE
JSR PC,CRLF
JMP $8
$5: MOV #7,R1 ;THIS IS HIGH BYTE ERROR
MOV #16,R0
$7: ASRB STATSV+1
BCC $6
ASL R0
MOV ERRMES(R0),SG
JSR PC,TYPSTR
JSR PC,CRLF
$6: INC R0
SOB R1,$7
$8: MOV DEVICM,R2 ;GET THE NUMBER OF SERVOS ATTACHED
BEQ ENDS4 ;BRANCH IF NONE ATTACHED
MOV #COM7,SG ;ELSE TYPE OUT THE DEVICE BLOCK CONTENTS
JSR PC,TYPSTR
MOV #DEVICM+4,R1 ;START WITH THIS WORD
DEVLP: MOV (R1)+,R0
MOV #IOBUF,SG
JSR PC,CVO
MOV #IOBUF,SG
JSR PC,TYPSTR
MOV #SPACE3,SG
JSR PC,TYPSTR
SOB R2,DEVLP
ENDS4: MOV #CLRSCR,SG
JSR PC,TYPSTR
RTS PC
;ROUTINE TO DISPLAY CURRENT REFERENCE POWER SUPPLY READINGS
REF: MOV #START3,JOBSA
JMP 1010
START3: MOV #COM5,SG ;TYPE OUT THE CALIB. REFERENCE READING
JSR PC,TYPSTR
MOV REFER1,R0 ;GET REF. READING
MOV #IOBUF,SG
JSR PC,CVI
MOVB #54,(SG)+ ;PUT IN A COMMA AND SPACE CHARACTER
MOVB #40,(SG)+
MOV REFER2,R0 ;GET SECOND REF. READING
JSR PC,CVI
MOV #IOBUF,SG ;TYPE THEM OUT
JSR PC,TYPSTR
JSR PC,CRLF
CLR DEVICE ;DON'T READ ANY TACHS
REFLP: MOV #DEVICE,R1 ;READ THE REF. POWER SUPPLY VOLTAGE
JSR PC,SETREF
MOV #REFSG2,SG ;START BUILDING OUTPUT STRING IN HERE
MOV REFVT1,R0 ;GET REFERENCE READINGS AND CONVERT TO ASC
JSR PC,CVI
MOVB #54,(SG)+ ;PUT IN A COMMA AND SPACE CHARACTER
MOVB #40,(SG)+
MOV REFVT2,R0 ;GET THE SECOND READING
JSR PC,CVI
MOVB #40,(SG)+ ;PUT IN A FEW SPACE CHARACTERS
MOVB #40,(SG)+
MOVB #40,(SG)+
MOV #REFSTG,SG ;TYPE OUT WHOLE STRING
JSR PC,TYPSTR
TST KBIS ;CHECK IF ANYONE HIT VT05 KEYBOARD
BEQ REFLP
CLRB KBIR
JSR PC,CRLF
DISMIS
;SECTION TO READ FORCE WRIST AND RETURN INFORMATION TO PDP10
FORCE: MOV #WSTADR,R1
MOV #10.,R2 ;READ 10 SETS OF DATA
SETLP: MOV #9.,R3 ;EIGHT STRAIN GAGES IN ALL+REF CHAN
MOV #30,R4 ;FIRST STRAIN GAGE CHANNEL
REDLP: MOVB R4,ADCCHN ;START CONVERTING STRAIN GAGE READING
WLP: TSTB ADCSR ;WAIT TILL CONVERSION COMPLETED
BMI CNVDNE
BR WLP
CNVDNE: MOV ADCVAL,R0 ;GET READING FROM BLUE INTERFACE
; ADD #2048.,R0
MOV R0,(R1)+ ;SAVE READING
INC R4 ;POINT TO NEXT CHANNEL
SOB R3,REDLP ;REPEAT UNTIL DONE
CLR -2(R1) ;NO REFERENCE READING
SOB R2,SETLP ;DO IT 10. TIMES
RTS PC
;FORCE SENSING/COMPLIANCE PATCH AREA
FPAT1: MOV #FTABLE+BLUARM,R0;INIT FORCE SENSING SYSTEM
MOV #C,R1 ;RELATIVE FORCE TRANS
JSR PC,SETC
TST R0 ;ERROR?
BEQ 1$
BPT
.IFNZ 0
1$: MOV #BLUARM+XFORCE,R0
LDF FFORCE,AC0
JSR PC,COMPLY
TST R0 ;ERROR?
BEQ 2$
BPT
2$: RTS PC
.BLKW 100
FPAT2: RTS PC
.BLKW 100
.IFF
1$: MOV FBITS,R0
MOV #FPDB,R1
MOV #FSSUB,R2
LDF FFORCE,AC0
JSR PC,FRCSIG
RTS PC
.BLKW 100
FPAT2: MOV FBITS,R0
MOV #FPDB,R1
JSR PC,FRCOFF
RTS PC
.BLKW 100
FSSUB: MOV #FMES,SG
JSR PC,TYPSTR
DISMIS
FBITS: BLUARM+XFORCE+SIGGE
FPDB: PDBLK 3,30,FP
FMES: .ASCIZ /
πFORCE SENSING TRIGGERED
/
.EVEN
.ENDC
FFORCE: .FLT2 0.0
C: .FLT2 1.0,0.0,0.0
.FLT2 0.0,1.0,0.0
.FLT2 0.0,0.0,1.0
.FLT2 0.0,0.0,0.0
;SIMULTANEOUS HAND OPERATION
.IFNZ HANDOP
SHAND: INC HANDGO
LDF CI+SRV14,AC0 ;PUT DYNAMIC COEF. IN DATA LIST
LDF CII+SRV14,AC1
STF AC0,HCI
STF AC1,HCII
MOV #HNDLST,R0 ;HAND COEFFICIENT LIST
MOV #HDEVIC,R1
JSR PC,MOVE
MOV #HMES,SG ;TYPE STATUS CODE
JSR PC,TYPSTR
MOV #HIOBUF,SG
JSR PC,CVO
MOV #HIOBUF,SG
JSR PC,TYPSTR
JSR PC,CRLF
CLR HANDGO
DISMISS
;LOCAL STORAGE AREA
HNDPDB: PDBLK 3,20,FP
HIOBUF: .BLKW 100
HDEVIC: .BLKW 10
HANDGO: 0
HNDLST: BHAND
0
0 ;NO SPECIAL MODES
0 ;NO WOBBLE
40. ;RELATIVE SEGMENT POINTER
5000. ;MOTION TIME
0 ;NO ASSOCIATED TRANS
0 ;NO RUN CODE
.FLT2 0.0
.FLT2 0.0
.FLT2 0.0
.FLT2 20.0
.FLT2 -30.0
.FLT2 12.0
HCI: .FLT2 0.0 ;CI
HCII: .FLT2 0.0 ;CII
0 ;NO OTHER SEGMENTS
HMES: .ASCIZ /
HAND FINISHED, STATUS = /
.EVEN
.ENDC
;LOCAL STORAGE AREA
DEVICE: .BLKW 33 ;DEVICE BLOCK FOR WHERE
DEVICM: .BLKW 33 ;DEVICE BLOCK FOR MOVE AND CENTER
IOBUF: .BLKW 400.
EANGLE: .BLKW 12.
PRTMES: 0
JMPFRC: 0
PRTDVB: 0
VT05: 0
STATSV: 0
;WHERE COEFFICIENT DATA LIST
WLST: 374 ;JOINT SERVO BIT
0
;OUTPUT STRINGS
; TOP OF VT05 SCREEN
COMHD: .BYTE 35,10,10,10,37,10,10,10
.IFNZ TRANS
.BYTE 32,10,10,10,32,10,10,10
.ENDC
.BYTE 0
HEADER: .ASCII / P O I N T Y
JT 1 JT 2 JT 3 JT 4 JT 5 JT 6 HAND
-45.0 -165.0 6.75 -395.0 -95.0 -110.0 -0.20 LOW STOPS
190.0 -50.0 33.00 205.0 95.0 200.0 3.80 HIGH STOPS
180.0 -90.0 14.00 -90.0 90.0 0.0 PARK POSITION
CURRENT POS
A-D READINGS
/
.IFNZ TRANS
.ASCII / X Y Z O A T/
.ENDC
.BYTE 36
.BYTE 0
COMWH: .BYTE 35,10,10,10,12,10,10,10,12,10,10,10,12,10,10,10,12,10,10,10,12,10,10,10,12,10,10,10
.BYTE 0
BADW: .ASCIZ /
SERVO ERROR WHILE ATTEMPTING TO DO A "WHERE", ERROR CODE = /
BADSTR: .ASCIZ /
ARM INITIALIZATION FAILED, ERROR CODE = /
COM5: .ASCIZ /
REF. POWER SUPPLY READINGS DURING CALIBRATION = /
REFSTG: .ASCII /CURRENT REFERENCE READINGS = /
REFSG2: .ASCII / /
.BYTE 15,10,10,10,0
SUCCES: .ASCIZ /
LAST MOTION COMMAND WAS SUCCESSFUL
/
CLRSCR: .BYTE 37,10,10,10
.BYTE 0
NOSCCS: .ASCIZ /
LAST MOTION COMMAND WAS UNSUCCESSFUL
/
SPACE3: .ASCIZ / /
.EVEN
ERRMES: 0
ERR1
ERR2
ERR3
ERR4
ERR5
ERR6
ERR7
ERR10
ERR11
ERR12
ERR13
ERR14
ERR15
ERR4H
ERR1K
ERR2K
ERR4K
ERR10K
ERR20K
ERR40K
ERR1: .ASCIZ /
COULD NOT ATTACH TO REQUESTED JOINT(S)/
ERR2: .ASCIZ /
INCORRECT NUMBER OF JOINTS REQUESTED TO BE DRIVEN/
ERR3: .ASCIZ /
WIPERS COULD NOT BE READ WITHIN THEIR OPERATING RANGE/
ERR4: .ASCIZ /
ARM SOLUTION DOES NOT EXIST/
ERR5: .ASCIZ /
UNKNOWN TOUCH SENSOR REQUESTED/
ERR6: .ASCIZ /
NO MORE FREE SLOTS IN TOUCH SENSOR EVENT LIST/
ERR7: .ASCIZ /
EITHER ARM POWER SUPPLY IS OFF OR ONE OF BRAKES IS NOT SET/
ERR10: .ASCIZ /
REFERENCE POWER SUPPLY OUT OF RANGE/
ERR11: .ASCIZ /
ZERO VELOCITY TACHOMETER READING OUT OF RANGE/
ERR12: .ASCIZ /
ATTEMPTED TO SWITCH ARMS WHILE FORCE SERVOING/
ERR13: .ASCIZ /
NO MORE FREE SLOTS IN FORCE SENSOR EVENT LIST/
ERR14: .ASCIZ /
NEED ALL 6 ARM JOINTS IN ORDER TO DO FORCE SENSING OR COMPLIANCE/
ERR15: .ASCIZ /
CAN'T FORCE SERVO MOTION WITHOUT POLYNOMIAL/
ERR4H: .ASCIZ /
JOINT IS DOWN, INOPERABLE/
ERR1K: .ASCIZ /
CATASTROPHIC A-D ERROR HAS OCCURRED/
ERR2K: .ASCIZ /
PANIC BUTTON WAS PUSHED/
ERR4K: .ASCIZ /
EXCESSIVE JOINT FORCE ERROR/
ERR10K: .ASCIZ /
FUNCTION TOOK TOO LONG TO EXECUTE/
ERR20K: .ASCIZ /
JOINT STOPPED BEYOND STOP LIMIT/
ERR40K: .ASCIZ /
NO ARM SOLUTION WHILE DOING FORCE COMPLIANCE/
COM7: .ASCIZ /
THE DEVICE BLOCK SERVO WORDS ARE AS FOLLOWS:
/
EVMES: .ASCII /TOUCH SENSOR EVENT TRIGGERED
/
.BYTE 7,0
ASKFRC: .ASCIZ /
DO FORCE COMPLIANCE (0,1) = /
.EVEN
;MAIN PROGRAM PROCESSOR DESCRIPTOR BLOCK
MAINL: PDBLK 1,100,FP
TEMP == .
. = JOBDAT ;MAIN PROGRAM DESCRIPTOR BLOCK
MAINL
. = JOBSA ;START OF MAIN PROGRAM
PSTART
. = JOBDM ;JOB DATA AREA MEMORY MAP
USRDM
. = TEMP ;RESTORE LOCATION COUNTER
.MSG LAST POINTY LOCATION
.END POINTY